package model.robot;

import model.virtualmap.OccupancyMap;
import model.debug.DebugPrinter;

import java.io.PipedInputStream;
import java.io.BufferedReader;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.io.PipedOutputStream;
import java.io.IOException;

import java.util.Arrays;
import java.util.StringTokenizer;

/**
 * Title    :   The Mobile Robot Explorer Simulation Environment v2.0
 * Copyright:   GNU General Public License as published by the Free Software Foundation
 * Company  :   Hanze University of Applied Sciences
 *
 * @author Dustin Meijer        (2012)
 * @author Alexander Jeurissen  (2012)
 * @author Davide Brugali       (2002)
 * @version 2.0
 */

public class MobileRobotAI implements Runnable {

	// These values have been copied from a test where the
	// double[] position was printed during each of the robot's facing directions.
	// For clarity : UP
	private static final int NORTH = 270;
	// For clarity : -->  aka RIGHT
	private static final int EAST = 360; //359.999999999999;
	// For clarity : DOWN
	private static final int SOUTH = 90; //89.99999999999908;
	// For clarity : <-- aka LEFT
	private static final int WEST = 180; //179.99999999999838;

	private static final int FORWARD = 0;
	private static final int RIGHT = 90;
	private static final int BEHIND = -180;
	private static final int LEFT = -90;

	private static final int MAX_VIEW_DISTANCE = 10;


	private final OccupancyMap map;
	private final MobileRobot robot;

	private boolean running;

	// These fields are meant to the help the AI processing.
	private PipedInputStream pipeIn;
	private BufferedReader input;
	private PrintWriter output;

	private String result = "";

	private double[] position;
	private double[] measures;


	private boolean firstPosition;
	private double startX;
	private double startY;


	public MobileRobotAI(MobileRobot robot, OccupancyMap map) {
		this.map = map;
		this.robot = robot;

		try {
			this.pipeIn = new PipedInputStream();
			this.input = new BufferedReader(new InputStreamReader(pipeIn));
			this.output = new PrintWriter(new PipedOutputStream(pipeIn), true);

			robot.setOutput(output);

			this.result = new String();
			this.position = new double[3];
			this.measures = new double[360];

			this.firstPosition = true;
			this.running = true;
		} catch (IOException ex) {
			System.err.println("MobileRobotAI : Something went wrong with initializing the IO.");
		}
	}

	/**
	 * In this method the gui.controller sends commands to the robot and its devices.
	 * At the moment all the commands are hardcoded.
	 * The exercise is to let the gui.controller make intelligent decisions based on
	 * what has been discovered so far. This information is contained in the OccupancyMap.
	 */
	public void run() {
		DebugPrinter.print("MobileRobotAI : Started running.");

		while (running) {
			try {

				scanSurroundingsWithLaser();
				scanSurroundingsWithSonar();
				courseOfAction();


                if(isMapComplete()){
                    robot.quit();
                }


			} catch (IOException ioe) {
				System.err.println("execution stopped");
				running = false;
			}
		}
		DebugPrinter.print("MobileRobotAI : Finished running.");

	}

	private void courseOfAction() throws IOException {

		DebugPrinter.print("MobileRobotAI : courseOfAction -> Started");

		// Position of robot
		int xCoordinate = (int) this.position[0] / map.getCellDimension();
		int yCoordinate = (int) this.position[1] / map.getCellDimension();

		DebugPrinter.print("MobileRobotAI : courseOfAction -> xCoordinate : " + xCoordinate);
		DebugPrinter.print("MobileRobotAI : courseOfAction -> yCoordinate : " + yCoordinate);

		int[] searchDirections = determineSearchDirection(FORWARD);

		DebugPrinter.print("MobileRobotAI : courseOfAction -> searchDirections");
		DebugPrinter.print("MobileRobotAI : Xdir , Ydir");
		DebugPrinter.print(Arrays.toString(searchDirections));

		int xSearchDirection = searchDirections[0];
		int ySearchDirection = searchDirections[1];

		boolean reachedEnd = false;

		char[][] knownMap = this.map.getGrid();

		int stepsToTake = 0;
		int x = xCoordinate;
		int y = yCoordinate;

		while (stepsToTake < MAX_VIEW_DISTANCE && !reachedEnd) {
			boolean rightWallFound = findWallToTheRightOfRobot(x, y);

			if (rightWallFound && knownMap[x][y] == OccupancyMap.EMPTY) {

				stepsToTake++;

			} else if (rightWallFound && knownMap[x][y] == OccupancyMap.UNKNOWN) {

				DebugPrinter.print("MobileRobotAI : courseOfAction -> xSearch : foundUNKNOWN");
				moveRobotForward(stepsToTake - 2);    // TODO : CHECK OFFSET
				reachedEnd = true;

			} else if (rightWallFound && knownMap[x][y] == OccupancyMap.OBSTACLE) {

				DebugPrinter.print("MobileRobotAI : courseOfAction -> xSearch : foundOBSTACLE");
				moveRobotForward(stepsToTake - 3);   // TODO : CHECK OFFSET
				rotateRobotLeft();
				reachedEnd = true;

			} else if (!rightWallFound && knownMap[x][y] == OccupancyMap.UNKNOWN) {

				DebugPrinter.print("MobileRobotAI : courseOfAction -> xSearch : foundUNKNOWN");
				moveRobotForward(stepsToTake - 2);   // TODO : CHECK OFFSET
				reachedEnd = true;

			} else if (!rightWallFound && knownMap[x][y] == OccupancyMap.EMPTY) {

				DebugPrinter.print("MobileRobotAI : courseOfAction -> xSearch : foundEMPTY");
				cornerRight(stepsToTake);
				reachedEnd = true;

			}

			x += xSearchDirection;
			y += ySearchDirection;
		}

		if (!reachedEnd) {
			moveRobotForward(stepsToTake - 2);
		}
	}

	private void cornerRight(int stepsBeforeCorner) throws IOException {
		moveRobotForward(stepsBeforeCorner + 3);
		scanSurroundingsWithSonar();
		scanSurroundingsWithLaser();

		// Position of robot
		int xCoordinate = (int) this.position[0] / map.getCellDimension();
		int yCoordinate = (int) this.position[1] / map.getCellDimension();

		boolean rightWallFound;

		rightWallFound = findWallToTheRightOfRobot(xCoordinate, yCoordinate);

		if (!rightWallFound) {
			rotateRobotRight();
			moveRobotForward(2); // TODO : CHECK OFFSET
		}


	}

	private boolean findWallToTheRightOfRobot(int xCoordinate, int yCoordinate) {
		// The double[] position consists of the following :
		// 0 -> X coordinate
		// 1 -> Y coordinate
		// 2 -> direction between 0 and 360

		boolean rightWallFound = true;

		DebugPrinter.print("MobileRobotAI : findWallToTheRightOfRobot -> Starting");
		DebugPrinter.print("MobileRobotAI : findWallToTheRightOfRobot -> xCoordinate : " + xCoordinate);
		DebugPrinter.print("MobileRobotAI : findWallToTheRightOfRobot -> yCoordinate : " + yCoordinate);

		int[] searchDirections = determineSearchDirection(RIGHT);
		int xSearchDirection = searchDirections[0];
		int ySearchDirection = searchDirections[1];

		DebugPrinter.print("MobileRobotAI : findWallToTheRightOfRobot -> xSearchDirection : " + xSearchDirection);
		DebugPrinter.print("MobileRobotAI : findWallToTheRightOfRobot -> ySearchDirection : " + ySearchDirection);

		char[][] knownMap = this.map.getGrid();
		boolean reachedEnd = false;

		int stepsUntilWall = 0;
		int x = xCoordinate;
		int y = yCoordinate;

		while (stepsUntilWall < 6 && !reachedEnd) {
			if (knownMap[x][y] == OccupancyMap.UNKNOWN) {
				reachedEnd = true;
				rightWallFound = false;
			} else if (knownMap[x][y] == OccupancyMap.OBSTACLE) {
				reachedEnd = true;
				rightWallFound = true; // Useless statement, but added for clarity.
			}
			stepsUntilWall++;

			x += xSearchDirection;
			y += ySearchDirection;
		}

		if (!reachedEnd) {
			rightWallFound = false;
		}

		DebugPrinter.print("");
		DebugPrinter.print("MobileRobotAI : findWallToTheRightOfRobot -> " + rightWallFound);
		DebugPrinter.print("");

		return rightWallFound;
	}

	private int[] determineSearchDirection(int lookDirection) {
		int currentDirection = roundToClosestDirection(this.position[2]);

		//    DebugPrinter.print("MobileRobotAI : determineSearchDirection -> currentDirection : " + currentDirection);
		//    DebugPrinter.print("MobileRobotAI : determineSearchDirection -> lookDirection : " + lookDirection);

		if (currentDirection == 360 && lookDirection > 0) {
			currentDirection = 0;
		}

		currentDirection += lookDirection;

		// DebugPrinter.print("MobileRobotAI : determineSearchDirection -> currentDirection + lookDirection : " + currentDirection );

		int[] searchDirections = new int[2];

		int xDirection;
		int yDirection;

		switch (currentDirection) {
			case NORTH:
				//      DebugPrinter.print("MobileRobotAI : determineSearchDirection -> case : NORTH ");

				xDirection = 0;
				yDirection = -1;
				searchDirections[0] = xDirection;
				searchDirections[1] = yDirection;

				DebugPrinter.print("");
				return searchDirections;

			case EAST:
				//    DebugPrinter.print("MobileRobotAI : determineSearchDirection -> case : EAST ");

				xDirection = 1;
				yDirection = 0;
				searchDirections[0] = xDirection;
				searchDirections[1] = yDirection;

				DebugPrinter.print("");
				return searchDirections;

			case SOUTH:
				//     DebugPrinter.print("MobileRobotAI : determineSearchDirection -> case : SOUTH ");

				xDirection = 0;
				yDirection = 1;
				searchDirections[0] = xDirection;
				searchDirections[1] = yDirection;

				DebugPrinter.print("");
				return searchDirections;

			case WEST:
				//     DebugPrinter.print("MobileRobotAI : determineSearchDirection -> case : WEST ");

				xDirection = -1;
				yDirection = 0;
				searchDirections[0] = xDirection;
				searchDirections[1] = yDirection;

				DebugPrinter.print("");
				return searchDirections;

			default:
				throw new IllegalArgumentException("The currentDirection : " + currentDirection + " is not a known direction.");
		}
	}

	private int roundToClosestDirection(double numToRound) {

		int nDif = (int) (NORTH - numToRound);
		int eDif = (int) (EAST - numToRound);
		int sDif = (int) (SOUTH - numToRound);
		int wDif = (int) (WEST - numToRound);
   /*
        DebugPrinter.print("");

        DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> DIF : NESW - numToRound  = Difference" );

		DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> nDif: " + "(" + NORTH + "-" + numToRound + ") = " + nDif);
		DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> eDif: " + "(" + EAST + "-" + numToRound + ") = " + eDif);
		DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> sDif: " + "(" + SOUTH + "-" + numToRound + ") = " + sDif);
		DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> wDif: " + "(" + WEST + "-" + numToRound + ") = " + wDif);
  */
		if (nDif < 2 && nDif > -2) {
			DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> returning NORTH");
			DebugPrinter.print("");
			return NORTH;

		} else if (eDif < 2 && eDif > -2 || eDif < 362 && eDif > 358) {   // Special or case : 360 == 0 in angle.
			DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> returning EAST");
			DebugPrinter.print("");
			return EAST;

		} else if (sDif < 2 && sDif > -2) {
			DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> returning SOUTH");
			DebugPrinter.print("");
			return SOUTH;

		} else if (wDif < 2 && wDif > -2) {
			DebugPrinter.print("MobileRobotAI : roundToClosestDirection -> returning WEST");
			DebugPrinter.print("");
			return WEST;

		} else {
			throw new IllegalArgumentException("the number provided is outside the predefined boundaries");
		}


	}

	private void rotateRobotLeft() throws IOException {
		DebugPrinter.print("MobileRobotAI : Rotating left");
		DebugPrinter.print("");
		robot.sendCommand("P1.ROTATELEFT 90");
		result = input.readLine();
	}

	private void rotateRobotRight() throws IOException {
		DebugPrinter.print("MobileRobotAI : Rotating right");
		DebugPrinter.print("");
		robot.sendCommand("P1.ROTATERIGHT 90");
		result = input.readLine();
	}

	private void moveRobotForward(int tiles) throws IOException {
		DebugPrinter.print("MobileRobotAI : Moving forward " + tiles);
		DebugPrinter.print("");
		this.robot.sendCommand("P1.MOVEFW " + tiles * map.getCellDimension());
		result = input.readLine();
	}

	private void scanSurroundingsWithLaser() throws IOException {
		this.robot.sendCommand("R1.GETPOS");
		this.result = input.readLine();
		this.parsePosition(result, position);

		this.robot.sendCommand("L1.SCAN");
		this.result = input.readLine();
		this.parseMeasures(result, measures);
		this.map.drawLaserScan(position, measures);

		if (position[0] != startX && position[1] != startY) {
			firstPosition = false;
		}

		if (!firstPosition) {
			startX = robot.getPlatform().getRobotPosition().getX();
			startY = robot.getPlatform().getRobotPosition().getY();
		}
	}

	private void scanSurroundingsWithSonar() throws IOException {
		this.robot.sendCommand("R1.GETPOS");
		this.result = input.readLine();
		this.parsePosition(result, position);

		this.robot.sendCommand("S1.SCAN");
		this.result = input.readLine();
		this.parseMeasures(result, measures);
		this.map.drawSonarScan(position, measures);

	}

	private void parsePosition(String value, double position[]) {
		int indexInit;
		int indexEnd;
		String parameter;

		indexInit = value.indexOf("X=");
		parameter = value.substring(indexInit + 2);
		indexEnd = parameter.indexOf(' ');
		position[0] = Double.parseDouble(parameter.substring(0, indexEnd));

		indexInit = value.indexOf("Y=");
		parameter = value.substring(indexInit + 2);
		indexEnd = parameter.indexOf(' ');
		position[1] = Double.parseDouble(parameter.substring(0, indexEnd));

		indexInit = value.indexOf("DIR=");
		parameter = value.substring(indexInit + 4);
		position[2] = Double.parseDouble(parameter);

		//DebugPrinter.print(" X = " + position[0]);
		//DebugPrinter.print(" Y = " + position[1]);
		//DebugPrinter.print(" DIR = " + position[2]);
	}

	private void parseMeasures(String value, double measures[]) {
		for (int i = 0; i < 360; i++) {
			measures[i] = 100.0;
		}
		if (value.length() >= 5) {
			value = value.substring(5);  // removes the "SCAN " keyword

			StringTokenizer tokenizer = new StringTokenizer(value, " ");

			double distance;
			int direction;
			while (tokenizer.hasMoreTokens()) {
				distance = Double.parseDouble(tokenizer.nextToken().substring(2));
				direction = (int) Math.round(Math.toDegrees(Double.parseDouble(tokenizer.nextToken().substring(2))));
				if (direction == 360) {
					direction = 0;
				}
				measures[direction] = distance;
				// Printing out all the degrees and what it encountered.
//				DebugPrinter.print("direction = " + direction + " distance = " + distance);
			}
		}
	}

	private boolean isMapComplete() {


        boolean completeMap = false;

        int xCoordinate = (int) position[0] / map.getCellDimension();
        int yCoordinate = (int) position[1] / map.getCellDimension();
        
        if( findWallToTheRightOfRobot(xCoordinate, yCoordinate) ){

            int[] searchDirections = determineSearchDirection(RIGHT);
            int xSearchDirection = searchDirections[0];
            int ySearchDirection = searchDirections[1];

            char[][] knownMap = this.map.getGrid();

            int x = xCoordinate;
            int y = yCoordinate;

            while(knownMap[x][y] != OccupancyMap.OBSTACLE){
                x += xSearchDirection;
                y += ySearchDirection;
            }

            boolean continueSearch = true;

            int startXCoordinate = x;
            int startYCoordinate = y;

            int currentXCoordinate = x;
            int currentYCoordinate = y;

            int previousXCoordinate = startXCoordinate;
            int previousYCoordinate = startYCoordinate;

            do{
                try{
                    int[] adjacentWallPieceCoordinates = findAdjacentWallPiece( currentXCoordinate, currentYCoordinate, previousXCoordinate, previousYCoordinate);

                    previousXCoordinate = currentXCoordinate;
                    previousYCoordinate = currentYCoordinate;

                    currentXCoordinate = adjacentWallPieceCoordinates[0];
                    currentYCoordinate = adjacentWallPieceCoordinates[1];

                }catch(IllegalArgumentException ex){
                    continueSearch = false;
                }
            }while((currentXCoordinate != startXCoordinate || currentYCoordinate != startYCoordinate) && continueSearch);

            if(continueSearch){
                completeMap = true;
            }
        }

        return completeMap;
        
	}

	private int[] findAdjacentWallPiece(int xCoordinate, int yCoordinate, int previousXCoordinate, int previousYCoordinate) {
		int[] toReturn = new int[2];

		char[][] knownMap = this.map.getGrid();

		if (xCoordinate > 0 && knownMap[xCoordinate - 1][yCoordinate] == OccupancyMap.OBSTACLE &&
				(xCoordinate - 1 != previousXCoordinate || yCoordinate != previousYCoordinate)) {

			toReturn[0] = xCoordinate - 1;
			toReturn[1] = yCoordinate;

		} else if (knownMap[xCoordinate + 1][yCoordinate] == OccupancyMap.OBSTACLE &&
				(xCoordinate + 1 != previousXCoordinate || yCoordinate != previousYCoordinate)) {

			toReturn[0] = xCoordinate + 1;
			toReturn[1] = yCoordinate;

		} else if (yCoordinate > 0 && knownMap[xCoordinate][yCoordinate - 1] == OccupancyMap.OBSTACLE &&
				(xCoordinate != previousXCoordinate || yCoordinate - 1 != previousYCoordinate)) {

			toReturn[0] = xCoordinate;
			toReturn[1] = yCoordinate - 1;

		} else if (knownMap[xCoordinate][yCoordinate + 1] == OccupancyMap.OBSTACLE &&
				(xCoordinate != previousXCoordinate || yCoordinate + 1 != previousYCoordinate)) {

			toReturn[0] = xCoordinate;
			toReturn[1] = yCoordinate + 1;

		} else {
			throw new IllegalArgumentException("Wall is not complete!");
		}

		return toReturn;
	}

}
